import copy
import math
import time
import os
from multiprocessing.shared_memory import SharedMemory
from multiprocessing import Process, Barrier, Event
import struct
import airsim
from dataclasses import dataclass
from airsim import Pose, Vector3r
import pandas as pd
from typing import List, Tuple
import pymap3d as pm
import numpy as np
import signal
import threading
from threading import Lock

conflict_set_lock=Lock()
# ORCA半平面
class Plane:
    def __init__(self, point: np.ndarray = np.zeros(3), normal: np.ndarray = np.zeros(3)):
        # 面上的一点
        self.point: np.ndarray = point
        # 面的法向量
        self.normal: np.ndarray = normal

# 无人机控制线程
class UavFly(Process):
    def __init__(self, number, goals, settings, fly_on_hand, radius_all, barrier,shm_name,warmup_cycles,initial_position):
        super(UavFly, self).__init__()
        # 当前无人机编号
        self.number = number
        self.goals = np.array(goals)
        self.settings = settings
        self.radius_all = radius_all
        # self.inv_time_horizon = 1 / settings[number].time_horizon
        self.duration = settings[number].time_horizon / 2
        self.is_running = True
        self.fly_on_hand = fly_on_hand
        self.barrier = barrier
        self.conflict_set=[]
        self.shm_name = shm_name
        self.warmup_cycles=warmup_cycles
        self.initial_position = initial_position
        self.second_controller=None



    # 结束当前线程
    def stop(self):
        self.is_running = False

    # 获取无人机速度和位置
    def get_multirotor_state(self, client, vehicle_name):
        state = client.getMultirotorState(vehicle_name=vehicle_name)
        position_x=state.kinematics_estimated.position.x_val
        position_y=state.kinematics_estimated.position.y_val
        position_z=state.kinematics_estimated.position.z_val
        velocity_x = state.kinematics_estimated.linear_velocity.x_val
        velocity_y = state.kinematics_estimated.linear_velocity.y_val
        velocity_z = state.kinematics_estimated.linear_velocity.z_val
        return np.array([position_x+self.initial_position[vehicle_name][0], position_y+self.initial_position[vehicle_name][1], position_z]), np.array([velocity_x, velocity_y, velocity_z])

    def worker(self):
        current_time = time.time()
        # 预热阶段（解决系统时间API初始延迟问题）
        for i in range(self.warmup_cycles):
            target_time = current_time + self.duration
            while time.time() < target_time:
                pass  # 忙等待
            current_time = target_time
            print(f"UAV {self.number}: 预热完成 {i + 1}/{self.warmup_cycles}")
        # 等待所有线程完成预热
        self.barrier.wait()

    def run(self):
        i=0
        # self.second_controller = SecondController(self.number, self.goals, self.settings, self.fly_on_hand, self.radius_all, self.duration,
        #                                           self.conflict_set)
        # self.second_controller.start()
        # global conflict_set_lock
        client = airsim.MultirotorClient()
        client.enableApiControl(True, vehicle_name=self.number)
        client.armDisarm(True, vehicle_name=self.number)
        client.takeoffAsync(vehicle_name=self.number).join()

        if self.number == self.fly_on_hand:
            client.moveToPositionAsync(self.goals[0][0], self.goals[0][1], self.goals[0][2], -1,
                                       vehicle_name=self.fly_on_hand).join()
            client.moveToPositionAsync(self.goals[1][0], self.goals[1][1], self.goals[1][2], -1,
                                       vehicle_name=self.fly_on_hand).join()
        # 如果是无人机
        while self.goals[0][2] < client.simGetGroundTruthKinematics(self.number).position.z_val:
            client.moveByVelocityAsync(0, 0, -1, self.duration, vehicle_name=self.number)
        shm = SharedMemory(name=self.shm_name)
        # self.worker()
        self.barrier.wait()
        while self.is_running:
            i+=1

            conflict_set=[]
            # 获得无人机的位置和速度信息
            state = client.getMultirotorState(vehicle_name=self.number)
            # 当前无人机的位置在airsim坐标系下位置和速度
            current_position,current_velocity = self.get_multirotor_state(client, vehicle_name=self.number)
            # 无人机距离目标点的距离，用来计算pref_velocity
            relative_position_goal = self.goals[1] - current_position
            # 无人机行驶时最佳速度
            pref_velocity = relative_position_goal / np.linalg.norm(relative_position_goal) * self.settings[
                self.number].max_speed
            pref_velocity_list = pref_velocity.tolist()
            # 获取当前区域内无人机的所有无人机名称
            vehicle_names = client.listVehicles()
            # 获取除当前无人机以外的其他无人机信息
            vehicle_names.remove(self.number)
            orca_planes = []
            for vehicle_name in vehicle_names:
                another_position, another_velocity = self.get_multirotor_state(client,vehicle_name=vehicle_name)
                relative_position=current_position - another_position
                relative_velocity = current_velocity - another_velocity
                dist = np.linalg.norm(relative_position)
                print("dish:", dist)
                print("predict",
                      np.linalg.norm(current_velocity) * self.settings[self.number].time_horizon + np.linalg.norm(
                          another_velocity) * self.settings[vehicle_name].time_horizon)
                # 当相对距离小于两架无人机结合起来的冲突预测距离
                if dist <= (np.linalg.norm(current_velocity) * self.settings[self.number].time_horizon + np.linalg.norm(another_velocity) * self.settings[vehicle_name].time_horizon):

                    # 添加无人机名称到冲突集中
                    conflict_set.append(vehicle_name)
                    plane = Plane()
                    # 两架无人机的碰撞半径，将两架无人机视为质点
                    combined_radius = self.radius_all[self.settings[self.number].model] + self.radius_all[
                        self.settings[vehicle_name].model]
                    dist_sq = dist ** 2
                    combined_radius_sq = combined_radius ** 2
                    a = dist_sq
                    b = relative_position.dot(relative_velocity)
                    c = relative_velocity.dot(relative_velocity) - np.linalg.norm(
                        np.cross(relative_position, relative_velocity)) ** 2 / (
                                dist_sq - combined_radius_sq)
                    if b ** 2 - a * c <= 0:
                        continue
                    t = (b + math.sqrt(b ** 2 - a * c)) / a
                    ww = relative_velocity - t * relative_position
                    ww_length = np.linalg.norm(ww)
                    plane.normal = ww / ww_length
                    u = (combined_radius * t - ww_length) * plane.normal
                    if vehicle_name == self.fly_on_hand:
                        # 针对有人机选择自己避让
                        plane.point = current_velocity + u
                    # 如果无人机在第二个碰撞层
                    else:
                        plane.point = current_velocity + 0.5 * u
                    orca_planes.append(plane)
            if orca_planes:
                new_velocity, plane_length = self.linear_program3(orca_planes, self.settings[self.number].max_speed,
                                                                  pref_velocity, False, current_velocity)
                new_velocity = new_velocity.tolist()
                client.moveByVelocityAsync(new_velocity[0], new_velocity[1], new_velocity[2],
                                           vehicle_name=self.number, duration=self.duration)
                print(f"UAV {self.number}: 第{i}轮 | 可能发生碰撞"
                      f"速度:{new_velocity}")
            else:
                client.moveByVelocityAsync(pref_velocity_list[0], pref_velocity_list[1],
                                           pref_velocity_list[2], vehicle_name=self.number,
                                           duration=self.duration)
                print(f"UAV {self.number}: 第{i}轮 | 不会发生碰撞"
                      f"速度:{pref_velocity}")
            # with conflict_set_lock:
            #     self.conflict_set=conflict_set
                # 精准等待到目标时间
            target_time, = struct.unpack('q', shm.buf[:8])
            while target_time==0:
                time.sleep(0.01)
                target_time, = struct.unpack('q', shm.buf[:8])
            while time.time() < target_time - 0.001:  # 提前1ms结束忙等待
                time.sleep(max(0, (target_time - time.time()) * 0.5))  # 动态调整等待
            while time.time() < target_time:
                time.sleep(max(0, (target_time - time.time()) * 0.5))  # 空转等待，确保精准
            actual_time = time.time()
            latency = actual_time - target_time
            print(f"UAV {self.number}: 第{i}轮 | "
                  f"目标: {target_time} | 实际: {actual_time} | "
                  f"延迟: {latency * 1000:.3f}ms")


    def linear_program1(self, planes: List[Plane], plane_no: int, line: Tuple[np.ndarray, np.ndarray], radius: float,
                        opt_velocity: np.ndarray, direction_opt: bool, result: np.ndarray) -> bool:
        # 求线与原点的最小距离
        dot_product = line[0].dot(line[1])

        discriminant = dot_product ** 2 + radius ** 2 - np.linalg.norm(line[0]) ** 2
        if discriminant < 0.0:
            return False

        sqrt_discriminant = np.sqrt(discriminant)
        t_left = -dot_product - sqrt_discriminant
        t_right = -dot_product + sqrt_discriminant
        for i in range(plane_no):
            numerator = (planes[i].point - line[0]).dot(planes[i].normal)
            denominator = line[1].dot(planes[i].normal)
            if abs(denominator) <= 1e-6:
                if numerator > 0.0:
                    return False
                else:
                    continue

            t = numerator / denominator
            if denominator >= 0.0:
                t_left = max(t_left, t)
            else:
                t_right = min(t_right, t)

            if t_left > t_right:
                return False
        if direction_opt:
            if opt_velocity.dot(line[1]) > 0.0:
                result[:] = line[0] + t_right * line[1]
            else:
                result[:] = line[0] + t_left * line[1]
        else:
            t = line[1].dot(opt_velocity - line[0])
            if t < t_left:
                result[:] = line[0] + t_left * line[1]
            elif t > t_right:
                result[:] = line[0] + t_right * line[1]
            else:
                result[:] = line[0] + t * line[1]

        return True

    def linear_program2(self, planes: List[Plane], plane_no: int, radius: float, opt_velocity: np.ndarray,
                        direction_opt: bool, result: np.ndarray) -> bool:
        # 计算平面与原点距离
        plane_dist = planes[plane_no].point.dot(planes[plane_no].normal)
        plane_dist_sq = plane_dist ** 2
        radius_sq = radius ** 2
        # 判断最大速度是否大于平面的直线距离
        if plane_dist_sq > radius_sq:
            return False
        # 计算平面圆的半径
        plane_radius_sq = radius_sq - plane_dist_sq
        # 计算平面圆的圆心
        plane_center = plane_dist * planes[plane_no].normal
        if direction_opt:
            plane_opt_velocity = opt_velocity - (opt_velocity.dot(planes[plane_no].normal)) * planes[plane_no].normal
            plane_opt_velocity_sq = math.pow(np.linalg.norm(plane_opt_velocity), 2)
            if plane_opt_velocity_sq <= 1e-6:
                result[:] = plane_center
            else:
                result[:] = plane_center + np.sqrt(plane_radius_sq / plane_opt_velocity_sq) * plane_opt_velocity
        else:
            # 将opt放大到与平面有交点
            plane_velocity = opt_velocity - opt_velocity.dot(planes[plane_no].normal) * planes[plane_no].normal
            result[:] = opt_velocity * (math.sqrt(plane_radius_sq) / np.linalg.norm(plane_velocity))
            # 就是结果超过最大速度
            if np.linalg.norm(result) > radius:
                plane_velocity_norm = plane_velocity / np.linalg.norm(plane_velocity)
                # 结果就是最大圆与平面的交点，并且里原始方向近的那个交点形成的向量
                result[:] = plane_center + math.sqrt(plane_radius_sq) * plane_velocity_norm
        for i in range(plane_no):

            # 判断当前result是否在当前plane的范围内 >0代表不在范围内，需要再判断
            if planes[i].normal.dot(planes[i].point - result[i]) > 0:
                # 计算两个平面的法向量的叉积
                cross_product = np.cross(planes[i].normal, planes[plane_no].normal)
                # 平面 planeNo 和 i（几乎）平行，因为平面 i 失败了，所以之前求出的平面 planeNo 也失败了
                if np.linalg.norm(cross_product) <= 1e-6:
                    return False
                # 平面planeNo上并垂直于交线的线
                line_normal = np.cross(cross_product, planes[plane_no].normal)
                line_point = planes[plane_no].point + (
                        ((planes[i].point - planes[plane_no].point).dot(planes[i].normal)) / (
                    line_normal.dot(planes[i].normal))) * line_normal
                if not self.linear_program1(planes, i, (line_point, cross_product / np.linalg.norm(cross_product)),
                                            radius, opt_velocity, direction_opt,
                                            result):
                    return False
        return True

    def linear_program3(self, planes: List[Plane], radius: float, opt_velocity: np.ndarray, direction_opt: bool,
                        new_velocity: np.ndarray) -> int:
        if direction_opt:
            new_velocity[:] = opt_velocity * radius
        elif (opt_velocity[0] * opt_velocity[0] + opt_velocity[1] * opt_velocity[1] + opt_velocity[2] * opt_velocity[
            2]) > radius * radius:
            new_velocity[:] = opt_velocity / np.linalg.norm(opt_velocity) * radius
        else:
            new_velocity[:] = opt_velocity
        for i, pl in enumerate(planes):
            # new_velocity 位于平面的外侧，不满足orca约束
            if pl.normal.dot(pl.point - new_velocity) > 0.0:
                temp_result = copy.deepcopy(new_velocity)

                if not self.linear_program2(planes, i, radius, opt_velocity, direction_opt, new_velocity):
                    new_velocity[:] = temp_result
                    return new_velocity, i

        return new_velocity, len(planes)


class report_velocity_and_relative_position(Process):
    def __init__(self, folder_path, goals, settings, radius_all):
        super().__init__()
        # 控制进程运行的事件
        self._stop_event = Event()
        # 记录无人机飞行数据的文件夹地址
        self.folder_path = folder_path
        # 无人机的目标地址
        self.goals = goals
        self.settings = settings
        self.radius_all = radius_all
        # 进程ID
        self.process_id = os.getpid()

    def stop(self):
        """停止数据收集进程"""
        print(f"收到停止信号，进程ID: {self.process_id}")
        self._stop_event.set()

    def _signal_handler(self, signum, frame):
        """信号处理函数"""
        print(f"收到信号 {signum}，准备停止进程")
        self.stop()

    def run(self):
        """进程主函数"""
        # 注册信号处理函数
        signal.signal(signal.SIGTERM, self._signal_handler)
        signal.signal(signal.SIGINT, self._signal_handler)

        client = airsim.MultirotorClient()
        # 获取所有无人机名称
        vehicle_names = client.listVehicles()
        if not vehicle_names:
            print("No vehicles found.")
            return
        print(f"开始收集无人机数据，进程ID: {self.process_id}")
        # 初始化数据结构
        data = {
            "position": {name: [] for name in vehicle_names},
            "velocity": {name: [] for name in vehicle_names},
            "relative": {}
        }

        # 初始化每对无人机之间的相对数据
        for i, name_a in enumerate(vehicle_names):
            for j, name_b in enumerate(vehicle_names):
                if i < j:
                    key = f"{name_a}_{name_b}"
                    data["relative"][key] = {
                        "distance": [],
                        "neighbor_dist": [],
                        "radius": []
                    }
        while not self._stop_event.is_set():
            # 获取所有无人机状态
            states = {}
            for name in vehicle_names:
                state = client.simGetGroundTruthKinematics(vehicle_name=name)
                pos = np.array([state.position.x_val, state.position.y_val, -state.position.z_val])
                vel = np.array([state.linear_velocity.x_val, state.linear_velocity.y_val, -state.linear_velocity.z_val])
                states[name] = {"pos": pos, "vel": vel}

            # 记录位置和速度
            for name, info in states.items():
                data["position"][name].append(info["pos"].tolist())
                data["velocity"][name].append(info["vel"].tolist())

            # 记录相对信息
            for i, name_a in enumerate(vehicle_names):
                for j, name_b in enumerate(vehicle_names):
                    if i < j:
                        key = f"{name_a}_{name_b}"
                        pos_a = states[name_a]["pos"]
                        pos_b = states[name_b]["pos"]
                        vel_a = states[name_a]["vel"]
                        vel_b = states[name_b]["vel"]

                        distance = np.linalg.norm(pos_b - pos_a)
                        radius = self.radius_all[self.settings[name_a].model] + self.radius_all[
                            self.settings[name_b].model]
                        neighbor_dist = np.linalg.norm(vel_a) * self.settings[name_a].time_horizon + np.linalg.norm(
                            vel_b) * self.settings[name_b].time_horizon
                        # 仅在冲突范围内记录数据
                        data["relative"][key]["distance"].append(distance)
                        data["relative"][key]["neighbor_dist"].append(neighbor_dist)
                        data["relative"][key]["radius"].append(radius)
            time.sleep(0.1)  # 控制采集频率

        # 写入 CSV 文件
        df = pd.DataFrame()

        # 添加位置信息
        for name in vehicle_names:
            df[f"{name}_x"] = [p[0] for p in data["position"][name]]
            df[f"{name}_y"] = [p[1] for p in data["position"][name]]
            df[f"{name}_z"] = [p[2] for p in data["position"][name]]
            df[f"{name}_vx"] = [v[0] for v in data["velocity"][name]]
            df[f"{name}_vy"] = [v[1] for v in data["velocity"][name]]
            df[f"{name}_vz"] = [v[2] for v in data["velocity"][name]]

        # 添加相对信息
        for key, rel_data in data["relative"].items():
            df[f"{key}_distance"] = rel_data["distance"]
            df[f"{key}_neighbor_dist"] = rel_data["neighbor_dist"]
            df[f"{key}_radius"] = rel_data["radius"]

        file_name = "datasource.csv"
        df.to_csv(f'{self.folder_path}/{file_name}', index=False)
        print(f"{file_name} 数据已成功写入")

# 进行第二次ORCA判断
class SecondController(threading.Thread):
    def __init__(self,number, goals, settings, fly_on_hand, radius_all,duration,collide_set,initial_position):
        threading.Thread.__init__(self)

        self.number = number
        self.goals = np.array(goals)
        self.settings = settings
        self.fly_on_hand = fly_on_hand
        self.radius_all = radius_all
        self.duration = duration
        self.is_running = True
        self.collide_set = collide_set
        self.initial_position = initial_position

    # 获取无人机经纬度和速度
    def get_multirotor_state(self, client, vehicle_name):
        state = client.getMultirotorState(vehicle_name=vehicle_name)
        position_x = state.kinematics_estimated.position.x_val
        position_y = state.kinematics_estimated.position.y_val
        position_z = state.kinematics_estimated.position.z_val
        velocity_x = state.kinematics_estimated.linear_velocity.x_val
        velocity_y = state.kinematics_estimated.linear_velocity.y_val
        velocity_z = state.kinematics_estimated.linear_velocity.z_val
        return np.array([position_x+self.initial_position[vehicle_name][0], position_y+self.initial_position[vehicle_name][1], position_z]), np.array([velocity_x, velocity_y, velocity_z])

    def run(self):
        global conflict_set_lock
        client = airsim.MultirotorClient()
        client.enableApiControl(True, vehicle_name=self.number)
            # 如果是有人机则直接进行飞行 模仿有人机的飞行路线
        while self.is_running:
            # 获得无人机的位置和速度信息
            state = client.getMultirotorState(vehicle_name=self.number)
            # 当前无人机的位置在airsim坐标系下的速度和位置
            current_position,current_velocity = self.get_multirotor_state(client, vehicle_name=self.number)
            # 无人机距离目标点的距离，用来计算pref_velocity
            relative_position_goal = self.goals[1] - current_position
            # 无人机行驶时最佳速度
            pref_velocity = relative_position_goal / np.linalg.norm(relative_position_goal) * self.settings[
                self.number].max_speed
            # pref_velocity_list = pref_velocity.tolist()
            # 获取当前区域内无人机的所有无人机名称
            orca_planes = []
            with conflict_set_lock:
                for vehicle_name in self.collide_set:
                    another_position, another_velocity = self.get_multirotor_state(client, vehicle_name=vehicle_name)
                    relative_position = current_position - another_position
                    relative_velocity = current_velocity - another_velocity
                    dist = np.linalg.norm(relative_position)
                    # 当相对距离小于两架无人机结合起来的冲突预测距离
                    if (dist <= np.linalg.norm(relative_velocity) * self.settings[self.number].time_horizon and
                            np.dot(relative_position, relative_velocity) / (
                                    dist * np.linalg.norm(another_velocity)) > 0):
                        plane = Plane()
                        # 两架无人机的碰撞半径，将两架无人机视为质点
                        combined_radius = self.radius_all[self.settings[self.number].model] + self.radius_all[
                            self.settings[vehicle_name].model]
                        dist_sq = dist ** 2
                        combined_radius_sq = combined_radius ** 2
                        a = dist_sq
                        b = relative_position.dot(relative_velocity)
                        c = relative_velocity.dot(relative_velocity) - np.linalg.norm(
                            np.cross(relative_position, relative_velocity)) ** 2 / (
                                    dist_sq - combined_radius_sq)
                        if b ** 2 - a * c < 0:
                            continue
                        t = (b + math.sqrt(b ** 2 - a * c)) / a
                        ww = relative_velocity - t * relative_position
                        ww_length = np.linalg.norm(ww)
                        plane.normal = ww / ww_length
                        u = (combined_radius * t - ww_length) * plane.normal
                        if vehicle_name == self.fly_on_hand:
                            # 针对有人机选择自己避让
                            plane.point = current_velocity + u
                        # 如果无人机在第二个碰撞层
                        else:
                            plane.point = current_velocity + 0.5 * u
                        orca_planes.append(plane)
                if orca_planes:
                    new_velocity, plane_length = self.linear_program3(orca_planes, self.settings[self.number].max_speed,
                                                                      pref_velocity, False, current_velocity)
                    new_velocity = new_velocity.tolist()
                    client.moveByVelocityAsync(new_velocity[0], new_velocity[1], new_velocity[2],
                                               vehicle_name=self.number, duration=self.duration)
                time.sleep(0.1)

    def linear_program1(self, planes: List[Plane], plane_no: int, line: Tuple[np.ndarray, np.ndarray], radius: float,
                        opt_velocity: np.ndarray, direction_opt: bool, result: np.ndarray) -> bool:
        # 求线与原点的最小距离
        dot_product = line[0].dot(line[1])

        discriminant = dot_product ** 2 + radius ** 2 - np.linalg.norm(line[0]) ** 2
        if discriminant < 0.0:
            return False

        sqrt_discriminant = np.sqrt(discriminant)
        t_left = -dot_product - sqrt_discriminant
        t_right = -dot_product + sqrt_discriminant
        for i in range(plane_no):
            numerator = (planes[i].point - line[0]).dot(planes[i].normal)
            denominator = line[1].dot(planes[i].normal)
            if abs(denominator) <= 1e-6:
                if numerator > 0.0:
                    return False
                else:
                    continue

            t = numerator / denominator
            if denominator >= 0.0:
                t_left = max(t_left, t)
            else:
                t_right = min(t_right, t)

            if t_left > t_right:
                return False
        if direction_opt:
            if opt_velocity.dot(line[1]) > 0.0:
                result[:] = line[0] + t_right * line[1]
            else:
                result[:] = line[0] + t_left * line[1]
        else:
            t = line[1].dot(opt_velocity - line[0])
            if t < t_left:
                result[:] = line[0] + t_left * line[1]
            elif t > t_right:
                result[:] = line[0] + t_right * line[1]
            else:
                result[:] = line[0] + t * line[1]

        return True

    def linear_program2(self, planes: List[Plane], plane_no: int, radius: float, opt_velocity: np.ndarray,
                        direction_opt: bool, result: np.ndarray) -> bool:
        # 计算平面与原点距离
        plane_dist = planes[plane_no].point.dot(planes[plane_no].normal)
        plane_dist_sq = plane_dist ** 2
        radius_sq = radius ** 2
        # 判断最大速度是否大于平面的直线距离
        if plane_dist_sq > radius_sq:
            return False
        # 计算平面圆的半径
        plane_radius_sq = radius_sq - plane_dist_sq
        # 计算平面圆的圆心
        plane_center = plane_dist * planes[plane_no].normal
        if direction_opt:
            plane_opt_velocity = opt_velocity - (opt_velocity.dot(planes[plane_no].normal)) * planes[plane_no].normal
            plane_opt_velocity_sq = math.pow(np.linalg.norm(plane_opt_velocity), 2)
            if plane_opt_velocity_sq <= 1e-6:
                result[:] = plane_center
            else:
                result[:] = plane_center + np.sqrt(plane_radius_sq / plane_opt_velocity_sq) * plane_opt_velocity
        else:
            # 将opt放大到与平面有交点
            plane_velocity = opt_velocity - opt_velocity.dot(planes[plane_no].normal) * planes[plane_no].normal
            result[:] = opt_velocity * (math.sqrt(plane_radius_sq) / np.linalg.norm(plane_velocity))
            # 就是结果超过最大速度
            if np.linalg.norm(result) > radius:
                plane_velocity_norm = plane_velocity / np.linalg.norm(plane_velocity)
                # 结果就是最大圆与平面的交点，并且里原始方向近的那个交点形成的向量
                result[:] = plane_center + math.sqrt(plane_radius_sq) * plane_velocity_norm
        for i in range(plane_no):

            # 判断当前result是否在当前plane的范围内 >0代表不在范围内，需要再判断
            if planes[i].normal.dot(planes[i].point - result[i]) > 0:
                # 计算两个平面的法向量的叉积
                cross_product = np.cross(planes[i].normal, planes[plane_no].normal)
                # 平面 planeNo 和 i（几乎）平行，因为平面 i 失败了，所以之前求出的平面 planeNo 也失败了
                if np.linalg.norm(cross_product) <= 1e-6:
                    return False
                # 平面planeNo上并垂直于交线的线
                line_normal = np.cross(cross_product, planes[plane_no].normal)
                line_point = planes[plane_no].point + (
                        ((planes[i].point - planes[plane_no].point).dot(planes[i].normal)) / (
                    line_normal.dot(planes[i].normal))) * line_normal
                if not self.linear_program1(planes, i, (line_point, cross_product / np.linalg.norm(cross_product)),
                                            radius, opt_velocity, direction_opt,
                                            result):
                    return False
        return True

    def linear_program3(self, planes: List[Plane], radius: float, opt_velocity: np.ndarray, direction_opt: bool,
                        new_velocity: np.ndarray) -> int:
        if direction_opt:
            new_velocity[:] = opt_velocity * radius
        elif (opt_velocity[0] * opt_velocity[0] + opt_velocity[1] * opt_velocity[1] + opt_velocity[2] * opt_velocity[
            2]) > radius * radius:
            new_velocity[:] = opt_velocity / np.linalg.norm(opt_velocity) * radius
        else:
            new_velocity[:] = opt_velocity
        for i, pl in enumerate(planes):
            # new_velocity 位于平面的外侧，不满足orca约束
            if pl.normal.dot(pl.point - new_velocity) > 0.0:
                temp_result = copy.deepcopy(new_velocity)

                if not self.linear_program2(planes, i, radius, opt_velocity, direction_opt, new_velocity):
                    new_velocity[:] = temp_result
                    return new_velocity, i

        return new_velocity, len(planes)

class PlotRouteLine(Process):
    def __init__(self, number, goal):
        super(PlotRouteLine, self).__init__()
        self.dt = 0.01
        self.is_running = True
        self.number = number
        self.initial_position = np.array([[goal[0][0]], [goal[0][1]], [0]])

    def run(self):
        client = airsim.MultirotorClient()
        UAV_state = client.simGetGroundTruthKinematics(vehicle_name=self.number)
        pos_start = np.array(
            [[UAV_state.position.x_val], [UAV_state.position.y_val], [UAV_state.position.z_val]]) + self.initial_position
        plot_last_pos = [airsim.Vector3r(pos_start[0, 0], pos_start[1, 0], pos_start[2, 0])]
        while self.is_running:
            UAV_state = client.simGetGroundTruthKinematics(vehicle_name=self.number)
            pos_now = np.array([[UAV_state.position.x_val], [UAV_state.position.y_val],
                                [UAV_state.position.z_val]]) + self.initial_position
            vel_now = np.array([[UAV_state.linear_velocity.x_val], [UAV_state.linear_velocity.y_val],
                                [UAV_state.linear_velocity.z_val]])
            plot_v_start = [airsim.Vector3r(pos_now[0, 0], pos_now[1, 0], pos_now[2, 0])]
            plot_v_end = pos_now + vel_now
            plot_v_end = [airsim.Vector3r(plot_v_end[0, 0], plot_v_end[1, 0], plot_v_end[2, 0])]
            client.simPlotArrows(plot_v_start, plot_v_end, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0])
            client.simPlotLineList(plot_last_pos + plot_v_start, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness=8,
                                   is_persistent=True)
            plot_last_pos = plot_v_start
            time.sleep(self.dt)

    def stop(self):
        self.is_running = False


# 用于出现相关无人机
def generate_uav(goals):
    client = airsim.MultirotorClient()
    for key, goal in goals.items():
        if key != "000":
            pose = Pose(Vector3r(goal[0][0], goal[0][1], 0))
            client.simAddVehicle(key, "simpleflight", pose)
            print("无人机" + key + "创建成功。")


@dataclass
class Settings:
    # 无人机最大速度
    max_speed: float
    # 无人机当前型号
    model: str
    # 无人机预测冲突的时间
    time_horizon: float

# 预热环节
class main_process(Process):
    def __init__(self, shm_name,barrier,warmup_cycles,duration):
        super(main_process, self).__init__()
        self.shm_name = shm_name
        self.barrier = barrier
        self.warmup_cycles=warmup_cycles
        self.duration=duration


    def worker(self):
        current_time = time.time()
        # 预热阶段（解决系统时间API初始延迟问题）
        for i in range(self.warmup_cycles):
            target_time = current_time + self.duration
            while time.time() < target_time:
                pass  # 忙等待
            current_time = target_time
            print(f"主程序: 预热完成 {i + 1}/{self.warmup_cycles}")
        # 等待所有线程完成预热
        self.barrier.wait()
    def run(self):
        i=0
        shm = SharedMemory(name=self.shm_name)
        # self.worker()
        self.barrier.wait()
        start_time = int(time.time())
        while True:
            i+=1
            #用于生成下一阶段目标时间
            cycle_number = int((time.time() - start_time) / self.duration) + 1
            target_time = start_time + cycle_number * self.duration

            shm.buf[:8] = struct.pack('q', target_time)
            while time.time() < target_time - 0.001:  # 提前1ms结束忙等待
                time.sleep(max(0, (target_time - time.time()) * 0.5))  # 动态调整等待
            while time.time() < target_time:
                time.sleep(max(0, (target_time - time.time()) * 0.5))  # 空转等待，确保精准
            actual_time = time.time()
            latency = actual_time - target_time
            print(f"主线程: 第{i}轮 | "
                  f"目标: {target_time} | 实际: {actual_time} | "
                  f"延迟: {latency * 1000:.3f}ms")

# 两架无人机
if __name__ == '__main__':
    # 飞行模式：若model=1则代表碰撞距离判断只有一个圈，若model=2则代表碰撞距离的判断有两个圈
    model = 1
    # 存放无人机飞行过程数据的文件夹
    folder_path = "UAV-HAV-5-2-25-5--3" + "model-" + str(model)
    if not os.path.exists(folder_path):
        os.makedirs(folder_path)
    # 有人机编号
    fly_on_hand = ""
    # 提前预测冲突的时间
    prediction_time = 4
    # 碰撞半径，根据不同无人机型号确定碰撞半径，“0”:3代表型号为0的无人机的碰撞半径为3，实际上无人机实际的机身半径小于这个值
    radius_all = {"0": 3, "1": 6, "2": 9}
    # 这里为设置每架无人机的设置
    settings_all = {"000": Settings(6, "0", prediction_time), "001": Settings(6, "0", prediction_time)}
    # 为无人机的位置设置，这个距离设置为在airsim的坐标系下的设置
    goals = {"000": [[0, 0, -10], [200, 200, -10]], "001": [[199, 201, -10], [-200, -200, -10]]}
    initial_position={"000":[0,0],"001":[199,201]}
    # 因为airsim启动时只有一架无人机，所以对于多出来的无人机需要进行创建
    generate_uav(goals)
    # 用来记录每架无人机飞行控制线程
    uav_flys = {}
    # 用来记录在airsim中绘制无人机轨迹的线程
    plot_route_lines = {}
    # reach_goal = reach_goals(folder_path, goals, settings_all,radius_all)
    # reach_goal.start()
    # 用来记录无人机飞行过程中的数据
    report_velocity_and_relative_positions = report_velocity_and_relative_position(folder_path, goals, settings_all,
                                                                                   radius_all)
    report_velocity_and_relative_positions.start()

    num_processes = len(settings_all)+1
    barrier = Barrier(num_processes)
    processes = []
    # 共享内存
    shm = SharedMemory(create=True, size=8)
    shm.buf[:8] = struct.pack('q', 0)
    # 预热循环次数
    warmup_cycles = 1
    # 预热持续时间
    duration = int(prediction_time/2)
    # 用来存入时间的线程
    processes.append(main_process(shm.name, barrier, warmup_cycles, duration))
    for key, goal in goals.items():
        process = UavFly(key, goal, settings_all, fly_on_hand, radius_all, barrier,shm.name,warmup_cycles,initial_position)
        processes.append(process)
        plot_route_lines[key] = PlotRouteLine(key, goal)
        plot_route_lines[key].start()
    # 判断“000”无人机是否到达目的地的线程
    for p in processes:
        p.start()
    # 定义信号处理函数
    try:
        # 等待进程结束
        report_velocity_and_relative_positions.join()
    except KeyboardInterrupt:
        print("收到键盘中断，正在停止进程...")
        # 发送SIGTERM信号
        os.kill(report_velocity_and_relative_positions.process_id, signal.SIGTERM)
        # 等待进程结束
        report_velocity_and_relative_positions.join()
